#include <zephyr.h>
#include <drivers/sensor.h>
#include <drivers/gpio.h>
#include <stdio.h>
#include "tensorflow/lite/micro/micro_mutable_op_resolver.h"
#include "tensorflow/lite/micro/micro_interpreter.h"

#define ACCEL_LABEL "ADXL375"
#define PRESSURE_LABEL "BMP388"
#define PIR_LABEL "AM312"

#define DOOR_ACTUATOR_GPIO 12
#define BUZZER_GPIO 13

static const struct device *accel_dev;
static const struct device *pressure_dev;
static const struct device *pir_dev;

const tflite::Model *model = nullptr;
tflite::MicroInterpreter *interpreter = nullptr;
TfLiteTensor *input = nullptr;
TfLiteTensor *output = nullptr;

int init_sensors(void) {
    accel_dev = device_get_binding(ACCEL_LABEL);
    pressure_dev = device_get_binding(PRESSURE_LABEL);
    pir_dev = device_get_binding(PIR_LABEL);
    
    if (!accel_dev || !pressure_dev || !pir_dev) {
        return -1;
    }
    return 0;
}

int init_actuators(void) {
    const struct device *gpio_dev = device_get_binding("GPIO_0");
    gpio_pin_configure(gpio_dev, DOOR_ACTUATOR_GPIO, GPIO_OUTPUT);
    gpio_pin_configure(gpio_dev, BUZZER_GPIO, GPIO_OUTPUT);
    return 0;
}

int load_ai_model(void) {
    
    extern const unsigned char g_collision_detection_model_data[];
    extern const int g_collision_detection_model_data_len;
    
    model = tflite::GetModel(g_collision_detection_model_data);
    if (model->version() != TFLITE_SCHEMA_VERSION) {
        return -1;
    }
    
    static tflite::MicroMutableOpResolver<5> resolver;
    resolver.AddFullyConnected();
    resolver.AddConv2D();
    resolver.AddMaxPool2D();
    resolver.AddReshape();
    resolver.AddSoftmax();
    
    static uint8_t tensor_arena[10 * 1024]; 
    static tflite::MicroInterpreter static_interpreter(
        model, resolver, tensor_arena, sizeof(tensor_arena));
    
    interpreter = &static_interpreter;
    if (interpreter->AllocateTensors() != kTfLiteOk) {
        return -2;
    }
    
    input = interpreter->input(0);
    output = interpreter->output(0);
    return 0;
}

void collision_detection_thread(void) {
    struct sensor_value accel[3], pressure, pir;
    float delta_p = 0.0f;
    float last_pressure = 0.0f;
    bool collision_detected = false;
    
    while (1) {
       
        sensor_sample_fetch(accel_dev);
        sensor_channel_get(accel_dev, SENSOR_CHAN_ACCEL_XYZ, accel);
        
        sensor_sample_fetch(pressure_dev);
        sensor_channel_get(pressure_dev, SENSOR_CHAN_PRESS, &pressure);
        
        sensor_sample_fetch(pir_dev);
        sensor_channel_get(pir_dev, SENSOR_CHAN_PROX, &pir);
        
       
        float accel_magnitude = sqrt(accel[0].val1*accel[0].val1 + 
                                    accel[1].val1*accel[1].val1 + 
                                    accel[2].val1*accel[2].val1);
        
        if (accel_magnitude > 20.0f) { // 20g阈值
            
            delta_p = fabs(pressure.val1 - last_pressure);
            if (delta_p > 5.0f) { // 5kPa变化
                collision_detected = true;
            }
        }
        
       
        if (collision_detected) {
          
            input->data.f[0] = accel[0].val1;
            input->data.f[1] = accel[1].val1;
            input->data.f[2] = accel[2].val1;
            input->data.f[3] = delta_p;
            input->data.f[4] = pir.val1;
            
          
            if (interpreter->Invoke() == kTfLiteOk) {
                if (output->data.f[0] > 0.8f) { // 置信度阈值
                    trigger_emergency_protocol();
                    collision_detected = false;
                }
            }
        }
        
        last_pressure = pressure.val1;
        k_sleep(K_MSEC(10)); // 10ms采样间隔
    }
}

void trigger_emergency_protocol(void) {
    const struct device *gpio_dev = device_get_binding("GPIO_0");
    
  
    gpio_pin_set(gpio_dev, DOOR_ACTUATOR_GPIO, 1);
    k_sleep(K_MSEC(500));
    gpio_pin_set(gpio_dev, DOOR_ACTUATOR_GPIO, 0);
    
    
    gpio_pin_set(gpio_dev, BUZZER_GPIO, 1);
    
    
    send_emergency_signal();
}

void main(void) {
    if (init_sensors() != 0) {
        printf("传感器初始化失败!\n");
        return;
    }
    
    if (init_actuators() != 0) {
        printf("执行机构初始化失败!\n");
        return;
    }
    
    if (load_ai_model() != 0) {
        printf("AI模型加载失败!\n");
        return;
    }
    
  
    k_thread_create(&collision_thread, collision_stack,
                    K_THREAD_STACK_SIZEOF(collision_stack),
                    (k_thread_entry_t)collision_detection_thread,
                    NULL, NULL, NULL,
                    K_PRIO_PREEMPT(5), 0, K_NO_WAIT);
    
    printf("AutoRescue-RV 系统已启动\n");
}